<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of kf</title>
  <meta name="keywords" content="kf">
  <meta name="description" content="KF  Kalman Filter">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; kf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>kf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>KF  Kalman Filter</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [xh, Px, pNoise, oNoise, InternalVariablesDS] = kf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> KF  Kalman Filter

   [xh, Px, pNoise, oNoise, InternalVariablesDS] = kf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         state                state mean at time k-1          ( xh(k-1) )
         Pstate               state covariance at time k-1    ( Px(k-1) )
         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          inference data structure generated by GENINFDS function.

   OUTPUT
         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
         Px                   state covariance
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

         InternalVariablesDS  (optional) internal variables data structure
           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
           .Px_                  predicted state covariance
           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )
           .inov                 inovation signal
           .Pinov                inovation covariance
           .KG                   Kalman gain</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
</ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="../.././ReBEL-0.2.7/examples/parameter_estimation/dempe1.html" class="code" title="">dempe1</a>	DEMPE1  Demonstrate parameter estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse1.html" class="code" title="">demse1</a>	DEMSE1  Demonstrate state estimation on a simple 2nd order LTI system.</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [xh, Px, pNoise, oNoise, InternalVariablesDS] = kf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% KF  Kalman Filter</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   [xh, Px, pNoise, oNoise, InternalVariablesDS] = kf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0010 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0013 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0014 <span class="comment">%   system.</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%   INPUT</span>
0017 <span class="comment">%         state                state mean at time k-1          ( xh(k-1) )</span>
0018 <span class="comment">%         Pstate               state covariance at time k-1    ( Px(k-1) )</span>
0019 <span class="comment">%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')</span>
0020 <span class="comment">%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')</span>
0021 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0022 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0023 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0024 <span class="comment">%         InferenceDS          inference data structure generated by GENINFDS function.</span>
0025 <span class="comment">%</span>
0026 <span class="comment">%   OUTPUT</span>
0027 <span class="comment">%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )</span>
0028 <span class="comment">%         Px                   state covariance</span>
0029 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0030 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%         InternalVariablesDS  (optional) internal variables data structure</span>
0033 <span class="comment">%           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )</span>
0034 <span class="comment">%           .Px_                  predicted state covariance</span>
0035 <span class="comment">%           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )</span>
0036 <span class="comment">%           .inov                 inovation signal</span>
0037 <span class="comment">%           .Pinov                inovation covariance</span>
0038 <span class="comment">%           .KG                   Kalman gain</span>
0039 
0040 <span class="comment">%   See also</span>
0041 <span class="comment">%   UKF, CDKF, EKF, GMSPKF, PF, SPPF, GMSPPF, GSPF</span>
0042 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0043 <span class="comment">%</span>
0044 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0045 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0046 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0047 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0048 <span class="comment">%</span>
0049 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0050 <span class="comment">%   detail.</span>
0051 
0052 <span class="comment">%=============================================================================================</span>
0053 
0054 
0055 Xdim  = InferenceDS.statedim;                                    <span class="comment">% state dimension</span>
0056 Odim  = InferenceDS.obsdim;                                      <span class="comment">% observation dimension</span>
0057 U1dim = InferenceDS.U1dim;                                       <span class="comment">% exogenous input 1 dimension</span>
0058 U2dim = InferenceDS.U2dim;                                       <span class="comment">% exogenous input 2 dimension</span>
0059 Vdim  = InferenceDS.Vdim;                                        <span class="comment">% process noise dimension</span>
0060 Ndim  = InferenceDS.Ndim;                                        <span class="comment">% observation noise dimension</span>
0061 
0062 NOV = size(obs,2);                                               <span class="comment">% number of input vectors</span>
0063 
0064 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0065 <span class="comment">%-- ERROR CHECKING</span>
0066 
0067 <span class="keyword">if</span> (nargin ~= 8) error(<span class="string">' [ kf ] Not enough input arguments!.'</span>); <span class="keyword">end</span>
0068 
0069 <span class="keyword">if</span> (Xdim~=size(state,1)) error(<span class="string">'[ kf ] Prior state dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0070 
0071 <span class="keyword">if</span> (Xdim~=size(Pstate,1)) error(<span class="string">'[ kf ] Prior state covariance dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0072 
0073 <span class="keyword">if</span> (Odim~=size(obs,1)) error(<span class="string">'[ kf ] Observation dimension differs from InferenceDS.obsdim'</span>); <span class="keyword">end</span>
0074 
0075 <span class="keyword">if</span> U1dim
0076     [dim,nop] = size(U1);
0077     <span class="keyword">if</span> (U1dim~=dim) error(<span class="string">'[ kf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'</span>); <span class="keyword">end</span>
0078     <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ kf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'</span>); <span class="keyword">end</span>
0079 <span class="keyword">end</span>
0080 
0081 <span class="keyword">if</span> U2dim
0082     [dim,nop] = size(U2);
0083     <span class="keyword">if</span> (U2dim~=dim) error(<span class="string">'[ kf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'</span>); <span class="keyword">end</span>
0084     <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ kf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'</span>); <span class="keyword">end</span>
0085 <span class="keyword">end</span>
0086 
0087 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0088 
0089 xh   = zeros(Xdim,NOV);
0090 xh_  = zeros(Xdim,NOV);
0091 yh_  = zeros(Odim,NOV);
0092 inov = zeros(Odim,NOV);
0093 
0094 <span class="keyword">if</span> (U1dim==0), UU1 = []; <span class="keyword">end</span>
0095 <span class="keyword">if</span> (U2dim==0), UU2 = []; <span class="keyword">end</span>
0096 
0097 <span class="keyword">if</span> pNoise.adaptMethod
0098     <span class="keyword">switch</span> InferenceDS.inftype
0099       <span class="keyword">case</span> <span class="string">'joint'</span>
0100           idx = pNoise.idxArr(<span class="keyword">end</span>,:);     <span class="comment">% get indeces of parameter block of combo-gaussian noise source</span>
0101           ind1 = idx(1);                  <span class="comment">% beginning index of parameter section</span>
0102           ind2 = idx(2);                  <span class="comment">% ending index of parameter section</span>
0103           edim = ind2-ind1+1;         <span class="comment">% infer parameter vector length</span>
0104           idxRange = ind1:ind2;
0105       <span class="keyword">case</span> <span class="string">'state'</span>
0106           ind1 = 1;
0107           ind2 = Xdim;
0108           edim = Xdim;
0109           idxRange = ind1:ind2;
0110     <span class="keyword">end</span>
0111 <span class="keyword">end</span>
0112 
0113 <span class="comment">%--------------------------------------- Loop over all input vectors --------------------------------------------</span>
0114 <span class="keyword">for</span> i=1:NOV,
0115 
0116     <span class="keyword">if</span> (U1dim) UU1 = U1(:,i); <span class="keyword">end</span>           <span class="comment">% get exogenous input</span>
0117     <span class="keyword">if</span> (U2dim) UU2 = U2(:,i); <span class="keyword">end</span>           <span class="comment">% get exogenous input</span>
0118 
0119     <span class="comment">%------------------------------------------------------</span>
0120     <span class="comment">%--- TIME UPDATE</span>
0121 
0122     A = InferenceDS.linearize( InferenceDS, state, pNoise.mu, [], UU1, [], <span class="string">'A'</span>);
0123     G = InferenceDS.linearize( InferenceDS, state, pNoise.mu, [], UU1, [], <span class="string">'G'</span>);
0124 
0125     xh_(:,i) = InferenceDS.ffun( InferenceDS, state, pNoise.mu, UU1);
0126     Px_      = A*Pstate*A' + G*pNoise.cov*G';
0127 
0128     <span class="comment">%--- MEASUREMENT UPDATE</span>
0129     C = InferenceDS.linearize( InferenceDS, xh_(:,i), [], oNoise.mu, [], UU2, <span class="string">'C'</span>);
0130     H = InferenceDS.linearize( InferenceDS, xh_(:,i), [], oNoise.mu, [], UU2, <span class="string">'H'</span>);
0131 
0132     Py        = C*Px_*C' + H*oNoise.cov*H';
0133     KG        = Px_ * C' * inv(Py);
0134 
0135     yh_(:,i)  = InferenceDS.hfun( InferenceDS, xh_(:,i), oNoise.mu, UU2);
0136 
0137     <span class="keyword">if</span> isempty(InferenceDS.innovation)
0138         inov(:,i) = obs(:,i) - yh_(:,i);
0139     <span class="keyword">else</span>
0140         inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0141     <span class="keyword">end</span>
0142 
0143     xh(:,i)   = xh_(:,i) + KG * inov(:,i);
0144     Px        = Px_ - KG*Py*KG';
0145 
0146     state  = xh(:,i);
0147     Pstate = Px;
0148 
0149 
0150     <span class="keyword">if</span> pNoise.adaptMethod <span class="keyword">switch</span> InferenceDS.inftype
0151     <span class="comment">%---------------------- UPDATE PROCESS NOISE SOURCE IF NEEDED --------------------------------------------</span>
0152     <span class="keyword">case</span> <span class="string">'parameter'</span>  <span class="comment">%--- parameter estimation</span>
0153         <span class="keyword">switch</span> pNoise.adaptMethod
0154         <span class="keyword">case</span> <span class="string">'anneal'</span>
0155             pNoise.cov = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov) , pNoise.adaptParams(2)));
0156         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0157             pNoise.cov = (1/pNoise.adaptParams(1)-1) * Pstate;
0158         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0159             nu = 1/pNoise.adaptParams(1);
0160             pNoise.cov = (1-nu)*pNoise.cov + nu*KG*(KG*inov*inov')';
0161             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0162         <span class="keyword">otherwise</span>
0163             error(<span class="string">' [kf]unknown process noise adaptation method!'</span>);
0164         <span class="keyword">end</span>
0165 
0166     <span class="keyword">case</span> <span class="string">'joint'</span>  <span class="comment">%--- joint estimation</span>
0167         <span class="keyword">switch</span> pNoise.adaptMethod
0168         <span class="keyword">case</span> <span class="string">'anneal'</span>
0169             pNoise.cov(idxRange,idxRange) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(idxRange,idxRange)), pNoise.adaptParams(2)));
0170         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0171             pNoise.cov(idxRange,idxRange) = (1/pNoise.adaptParams(1)-1) * Pstate(end-edim+1:<span class="keyword">end</span>,end-edim+1:end);
0172         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0173             nu = 1/pNoise.adaptParams(1);
0174             subKG = KG(end-edim+1:<span class="keyword">end</span>,:);
0175             pNoise.cov(idxRange,idxRange) = (1-nu)*InferenceDS.pNoise.cov(idxRange,idxRange) + nu*subKG*(subKG*inov*inov')';
0176             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0177         <span class="keyword">otherwise</span>
0178             error(<span class="string">' [kf]unknown process noise adaptation method!'</span>);
0179         <span class="keyword">end</span>
0180 
0181     <span class="comment">%--------------------------------------------------------------------------------------------------</span>
0182     <span class="keyword">end</span>; <span class="keyword">end</span>
0183 
0184 
0185 <span class="keyword">end</span>   <span class="comment">%--- for loop</span>
0186 
0187 <span class="keyword">if</span> (nargout&gt;4),
0188   InternalVariablesDS.xh_   = xh_;
0189   InternalVariablesDS.Px_   = Px_;
0190   InternalVariablesDS.yh_   = yh_;
0191   InternalVariablesDS.inov  = inov;
0192   InternalVariablesDS.Pinov = Py;
0193   InternalVariablesDS.KG    = KG;
0194 <span class="keyword">end</span>
0195 
0196</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>